#ifndef TRAJECTORY_POINTS_HPP
#define TRAJECTORY_POINTS_HPP

#include <vector>
#include <memory>
#include <atomic>
#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <geometry_msgs/msg/pose.hpp>

#include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <atomic>
#include <thread>
#include <chrono> // 添加 chrono 头文件
#include <rclcpp/executors/multi_threaded_executor.hpp>
#include <std_msgs/msg/int32.hpp> // 添加 Int32 消息头文件
#include <limits>                 // 需要包含 <limits> 以使用 std::numeric_limits
#include "/home/jetson/zc6/src/servo_visual/doc/examples/realtime_servo/src/visual_control.cpp"
#include <std_msgs/msg/float64.hpp>
#include <algorithm>
#include <vector>

namespace path_plan {

inline std::vector<geometry_msgs::msg::Pose> getTrajectoryPoints() {
    /* #region  */
    std::vector<geometry_msgs::msg::Pose> waypoints;
    // 目标点0 28/-45/90/-45/0
    geometry_msgs::msg::Pose pose0;
    pose0.position.x = 0.083024;
    pose0.position.y = -0.10987;
    pose0.position.z = 0.28763;
    pose0.orientation.x = 0;
    pose0.orientation.y = 0;
    pose0.orientation.z = -0.24475;
    pose0.orientation.w = 0.96959;
    waypoints.push_back(pose0);

    // 目标点1 28/-50/90/15/0
    geometry_msgs::msg::Pose pose1;
    pose1.position.x = -0.034461;
    pose1.position.y = -0.046522;
    pose1.position.z = 0.10748;
    pose1.orientation.x = 0.11163;
    pose1.orientation.y = 0.44231;
    pose1.orientation.z = -0.21777;
    pose1.orientation.w = 0.86283;
    waypoints.push_back(pose1);

    // 目标点2 28/-60/100/10/0
    geometry_msgs::msg::Pose pose2;
    pose2.position.x = -0.033193;
    pose2.position.y = -0.047216;
    pose2.position.z = 0.096255;
    pose2.orientation.x = 0.10337;
    pose2.orientation.y = 0.40969;
    pose2.orientation.z = -0.22182;
    pose2.orientation.w = 0.87879;
    waypoints.push_back(pose2);

    // 目标点3 28/-65/110/0/0
    geometry_msgs::msg::Pose pose3;
    pose3.position.x = -0.016352;
    pose3.position.y = -0.056293;
    pose3.position.z = 0.087686;
    pose3.orientation.x = 0.093294;
    pose3.orientation.y = 0.36973;
    pose3.orientation.z = -0.22624;
    pose3.orientation.w = 0.89633;
    waypoints.push_back(pose3);

    // 目标点4 28/-70/119/-9/0
    geometry_msgs::msg::Pose pose4;
    pose4.position.x = -0.00072952;
    pose4.position.y = -0.064723;
    pose4.position.z = 0.083604;
    pose4.orientation.x = 0.082545;
    pose4.orientation.y = 0.32711;
    pose4.orientation.z = -0.23041;
    pose4.orientation.w = 0.91274;
    waypoints.push_back(pose4);

    // 目标点5 28/-78/134/-28/0 
    geometry_msgs::msg::Pose pose5;
    pose5.position.x = 0.030862;
    pose5.position.y = -0.081752;
    pose5.position.z = 0.082453;
    pose5.orientation.x = 0.059886;
    pose5.orientation.y = 0.23743;
    pose5.orientation.z = -0.23731;
    pose5.orientation.w = 0.94007;
    waypoints.push_back(pose5);

    // 目标点55 28/-78/136/-37/0 
    geometry_msgs::msg::Pose pose55;
    pose55.position.x = 0.045914;
    pose55.position.y = -0.08981;
    pose55.position.z = 0.10271;
    pose55.orientation.x = 0.045323;
    pose55.orientation.y = 0.17931;
    pose55.orientation.z = -0.24037;
    pose55.orientation.w = 0.9529;
    waypoints.push_back(pose55);

    // 目标点6 28/-78/138/-44/0
    geometry_msgs::msg::Pose pose6;
    pose6.position.x = 0.054854;
    pose6.position.y = -0.094613;
    pose6.position.z = 0.11793;
    pose6.orientation.x = 0.034817;
    pose6.orientation.y = 0.13775;
    pose6.orientation.z = -0.24206;
    pose6.orientation.w = 0.9598;
    waypoints.push_back(pose6);

    // 目标点7 28/-82/142/-52/0 半举平
    geometry_msgs::msg::Pose pose7;
    pose7.position.x = 0.061581;
    pose7.position.y = -0.098223;
    pose7.position.z = 0.14013;
    pose7.orientation.x = 0.017288;
    pose7.orientation.y = 0.068398;
    pose7.orientation.z = -0.24388;
    pose7.orientation.w = 0.96723;
    waypoints.push_back(pose7);

    // 目标点8 28/-82/145/-60/0 举平
    // 目标点8 28/-82/145/-58/0 举平
    geometry_msgs::msg::Pose pose8;
    pose8.position.x = 0.067408;
    pose8.position.y = -0.10135;
    pose8.position.z = 0.14599;
    pose8.orientation.x = 0.010242;
    pose8.orientation.y = 0.040522;
    pose8.orientation.z = -0.24425;
    pose8.orientation.w = 0.96881;
    waypoints.push_back(pose8);

    // 目标点8 28/-90/142/-50/0 刮板
    geometry_msgs::msg::Pose pose9;
    pose9.position.x = 0.053625;
    pose9.position.y = -0.093921;
    pose9.position.z = 0.16204;
    pose9.orientation.x = 0.0035733;
    pose9.orientation.y = 0.014151;
    pose9.orientation.z = -0.24446;
    pose9.orientation.w = 0.96955;
    waypoints.push_back(pose9);

    //第10个目标位姿 14/-90/142/-50/0 左移
    geometry_msgs::msg::Pose pose10;
    pose10.position.x = 0.071307;
    pose10.position.y = -0.047553;
    pose10.position.z = 0.16428;
    pose10.orientation.x = 0.0011452;
    pose10.orientation.y = 0.0093179;
    pose10.orientation.z = -0.12094;
    pose10.orientation.w = 0.99262;
    waypoints.push_back(pose10);

    //第11个目标位姿 0/-60/120/-60/0 low-middle
    geometry_msgs::msg::Pose pose11;
    pose11.position.x = 0.11027;
    pose11.position.y = 0;
    pose11.position.z = 0.226;
    pose11.orientation.x = 0;
    pose11.orientation.y = 0;
    pose11.orientation.z = 0;
    pose11.orientation.w = 1;
    waypoints.push_back(pose11);

/* #endregion */


    return points;
}

inline geometry_msgs::msg::Pose getMiddlePose() {
    geometry_msgs::msg::Pose pose;
    pose.position.x = 0.09971029207890635;
    pose.position.y = 2.6285536383516047e-05;
    pose.position.z = 0.20181001562448328;
    pose.orientation.x = 1.8365485104639237e-06;
    pose.orientation.y = -3.3663826869417e-06;
    pose.orientation.z = 7.152014390535778e-05;
    pose.orientation.w = 0.9999999974350818;
    return pose;
}

struct CombinedNodeMembers {
    std::shared_ptr<VisualControl> visual_control_node;
    std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group;
    rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr force_sub;
    rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr result_sub;
    std::atomic<bool> force_sign {true};
    std::atomic<size_t> closest_index {0};
    std::atomic<bool> is_executing {false};
    std::atomic<double> current_z_force {0.0};
};

} // namespace path_plan

#endif // TRAJECTORY_POINTS_HPP
